#!/usr/bin/env python3
# -*- coding:utf-8 -*-
# @Author: renjin@bit.edu.cn
# @Date  : 2024-08-07

"""
【节点名称】：
    CameraNode
【依赖项安装】：
    pip install spirems
【订阅类型】：
    无
【发布类型】：
    sensor_msgs::CompressedImage （输出图像）
    sensor_msgs::CameraCalibration （暂未实现）
【构造参数说明】：
    parameter_file (str): 全局参数文件
【节点参数】：
    resize (list): 图像重采样分辨率
    camera_id (int): 相机ID号
【备注】：
    无
"""

import threading
import time
import cv2
from typing import Union
from spirems import Publisher, cvimg2sms
from spirecv.base.BaseNode import BaseNode


class RTSPReaderNode(threading.Thread, BaseNode):
    def __init__(
        self,
        job_name: str,
        ip: str = '127.0.0.1',
        port: int = 9094,
        param_dict_or_file: Union[dict, str] = None,
        rtsp_url: str = ''
    ):
        threading.Thread.__init__(self)
        BaseNode.__init__(
            self,
            self.__class__.__name__,
            job_name,
            ip=ip,
            port=port,
            param_dict_or_file=param_dict_or_file,
            multi_instance_node=True
        )
        self._image_writer = Publisher(
            '/' + job_name + '/sensor/image_raw', 'sensor_msgs::CompressedImage',
            ip=ip, port=port
        )

        self.rtsp_url = self.get_param("rtsp_url", rtsp_url)
        self.resize = self.get_param("resize", [1280, 720])
        self.capture = cv2.VideoCapture(self.rtsp_url)
        self.frame_id = 0
        self.start()

    def release(self):
        BaseNode.release(self)
        self._image_writer.kill()
        self._image_writer.join()

    def run(self):
        while self.is_running():
            ret, frame = self.capture.read()
            if not ret:
                continue
            frame = cv2.resize(frame, self.resize)
            print(frame.shape)
            msg = cvimg2sms(frame)
            self.frame_id += 1
            msg['img_id'] = self.frame_id
            self._image_writer.publish(msg)

        self.release()
        print('{} quit!'.format(self.__class__.__name__))


if __name__ == '__main__':
    node1 = RTSPReaderNode("left", rtsp_url="rtsp://192.168.88.65:554/profile1")
    node2 = RTSPReaderNode("right", rtsp_url="rtsp://192.168.88.64:554/profile1")
